/*
 * usbcomm.c
 *
 *  Created on: Mar 9, 2012
 *      Author: Victor Nidens victornlj@gmail.com
 *
 *	This file is part of API LEGO for CMLab based on LEGO NXT Direct Commands API
 *	by Michael Collins michaelcollins@ivorycity.com and
 *	Aran Deltac aran@arandeltac.com
 *
 *	API LEGO for CMLab is free software: you can redistribute it and/or modify
 *	it under the terms of the GNU Lesser General Public License as published by
 *	the Free Software Foundation, either version 3 of the License, or
 *	(at your option) any later version.
 *
 *	API LEGO for CMLab library is distributed in the hope that it will be useful,
 *	but WITHOUT ANY WARRANTY; without even the implied warranty of
 *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *	GNU Lesser General Public License for more details.
 *
 *	You should have received a copy of the GNU Lesser General Public License
 *	along with API LEGO for CMLab. If not, see <http://www.gnu.org/licenses/>.
 *
 */

#include "usbcomm.h"
#include "libusb-1.0/libusb.h"

#include "stdio.h"

libusb_device **list;
libusb_device *found = NULL;

/**
 * Tries to init USB library with default libusb context
 * 
 * @return 
 */
int initUSB(){
	return libusb_init(NULL);
}

void freeUSB(libusb_device_handle **deviceHandle){

	libusb_free_device_list(list, 1);

	libusb_close(*deviceHandle);
}

/**
 * Searches through device list for USB device with given VendorID and ProductID
 *
 * @return Pointer to libusb_device object if device found, NULL otherwise
 */
libusb_device* findDevice(libusb_device **list, uint8 size, uint16 vendorId, uint16 productId){

	uint8 ind = 0;
	libusb_device *device = NULL;
	struct libusb_device_descriptor devDesciption;

	for(; ind < size; ind++){
		device = list[ind];
		libusb_get_device_descriptor(device, &devDesciption);


		if(devDesciption.idVendor == vendorId && devDesciption.idProduct == productId) {
			return device;
		}
	}

	return NULL;
}

libusb_device** findDevices(libusb_device **list, uint8 size, uint16 vendorId, uint16 productId, libusb_device **resultList, uint8 *resultSize){

	uint8 ind = 0;
	uint8 maxResultSize = *resultSize;
	uint8 devicesFound = 0;
	libusb_device *device = NULL;
	struct libusb_device_descriptor devDesciption;

	*resultSize = 0;

	for(; ind < size && devicesFound < maxResultSize; ind++){
		device = list[ind];
		libusb_get_device_descriptor(device, &devDesciption);

		if(devDesciption.idVendor == vendorId && devDesciption.idProduct == productId) {
			resultList[devicesFound] = device;
			devicesFound++;
		}
	}

	*resultSize = devicesFound;

	return resultList;
}

/**
 * Searches for the LEGO NXT device and connects to it
 * 
 * @return 0 if device was found and succesfully connected
 */
int connectNXTDevice(libusb_device_handle **deviceHandle){
	int ret = 0;

	ssize_t deviceCount;
    
	deviceCount = libusb_get_device_list(NULL, &list);

	if (deviceCount < 0){
		return (int) deviceCount;
	}

	found = findDevice(list, deviceCount, USB_ID_VENDOR_LEGO, USB_ID_PRODUCT_NXT);

	if (found) {

		ret = libusb_open(found, deviceHandle);
		if (ret) {
			return ret;
		}

		ret = libusb_reset_device(*deviceHandle);

		if (ret) {
			return ret;
		}

		ret = libusb_claim_interface(*deviceHandle, 0);

		if (ret) {
			return ret;
		}

		printf("LEGO device found\n");

	} else {
		printf("No LEGO device found\n");
	}

	return ret;
    
}

int connectNXTDevices(libusb_device_handle **deviceHandle, uint8 *devicesToConnect){
	int ret = 0;

	ssize_t deviceCount;

	libusb_device *foundedDevicesList[*devicesToConnect];
	uint8 devicesFound = *devicesToConnect;
	uint8 ind = 0;

	deviceCount = libusb_get_device_list(NULL, &list);

	if (deviceCount < 0){
		return (int) deviceCount;
	}

	findDevices(list, deviceCount, USB_ID_VENDOR_LEGO, USB_ID_PRODUCT_NXT, foundedDevicesList, devicesToConnect);

	devicesFound = *devicesToConnect;

	if(devicesFound < 1){
		return -1;
	}

	for(; ind < devicesFound; ind++) {

		found = foundedDevicesList[ind];

		if (found) {
			ret = libusb_open(found, &deviceHandle[ind]);
			if (ret) {
				return ret;
			}

			ret = libusb_reset_device(deviceHandle[ind]);

			if (ret) {
				return ret;
			}

			ret = libusb_claim_interface(deviceHandle[ind], 0);

			if (ret) {
				return ret;
			}

		}

	}

	return ret;
}

int executeCommand(libusb_device_handle **deviceHandle, uint8 *requestBuffer, uint8 requestLength, uint8 responseRequired, uint8 *responseBuffer, int *responseLength){
    
	int ret = 0;
	int transferredLength = 0;

	if(*deviceHandle == NULL){
		return -17;
	}

	if(requestBuffer == NULL || requestLength == 0){
		return -19;
	}

	ret = libusb_bulk_transfer(*deviceHandle, USB_OUT_ENDPOINT, requestBuffer, requestLength, &transferredLength, 0);

	if(responseRequired){
		ret = libusb_bulk_transfer(*deviceHandle, USB_IN_ENDPOINT, responseBuffer, *responseLength, responseLength, 100);
	}

	return ret;

}
